import socket
import os
import subprocess
import select
import struct
import time
import sys

# Source ROS环境
ros_setup_path = "/opt/ros/noetic/setup.bash"
mavros_ws_setup_path = "/home/wushuanglang/mavros_ws/devel/setup.bash"
# 动态 source setup.bash
command = ['bash', '-c', f"source {ros_setup_path} && source {mavros_ws_setup_path} && env"]
proc = subprocess.Popen(command, stdout=subprocess.PIPE)
for line in proc.stdout:
    (key, _, value) = line.decode("utf-8").partition("=")
    os.environ[key] = value.strip()

import rospy
from std_msgs.msg import String
from flydatas_msgs.msg import Flydatas
from sensor_msgs.msg import NavSatFix
from mavros_msgs.msg import State
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header

# 定义服务器IP和端口
server_ip = '127.0.0.1'
server_port = 6666

# 创建 TCP socket
client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.setblocking(False)  # 设置为非阻塞模式

# 定义变量来存储接收的数据，初始值全部为0.0，为0.0时为无效值，对于没使用到的值应全部置0.0
get_check1 = 0.0       # 包头（888.0有效）
get_speed = 0.0       # 速度
get_mode = 0.0        # 飞行模式/任务：1.0表示GPS巡航模式，2.0表示飞机控制模式，3.0表示...
get_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
get_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
get_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
get_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
get_check2 = 0.0       # 包尾（666.0有效）

# 定义变量来存储要发送的数据，初始值全部为0.0，为0.0时为无效值，对于没使用到的值应全部置0.0
send_check1 = 888.0     # 包头，888.0有效
send_speed = 0.0
send_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
send_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
send_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
send_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
send_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
send_check2 = 666.0     # 包尾，666.0有效

def send_datas_init():
    global send_check1
    global send_speed
    global send_mode
    global send_doit
    global send_longitude
    global send_latitude
    global send_altitude
    global send_check2
    send_check1 = 888.0     # 包头，888.0有效
    send_speed = 0.0
    send_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
    send_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
    send_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
    send_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
    send_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
    send_check2 = 666.0     # 包尾，666.0有效

def get_datas_to_0():
    global get_check1
    global get_speed
    global get_mode
    global get_doit
    global get_longitude
    global get_latitude
    global get_altitude
    global get_check2
    get_check1 = 0.0     # 包头，888.0有效
    get_speed = 0.0
    get_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
    get_doit = 0.0        # 执行确认位(1.0表示有效，0.0表示无效)
    get_longitude = [0.0,0.0,0.0,0.0]   # 新建一个列表来存储经度,longitude[0]和latitude[0]为一对
    get_latitude =  [0.0,0.0,0.0,0.0]   # 列表存储纬度
    get_altitude =  [0.0,0.0,0.0,0.0]   # 列表存储海拔（高度）
    get_check2 = 0.0     # 包尾，666.0有效

# socket发送数据
def send_data(client_socket, data_to_send):
    """发送数据到服务器"""
    try:
        # 使用 struct 打包数据，17个 double 类型
        packed_data = struct.pack('>17d', *data_to_send)
        # 发送打包的数据到服务器
        client_socket.sendall(packed_data)
        print(f"Sent data: {data_to_send}")
    except BlockingIOError:
        print("发送数据时阻塞，请稍后再试。")
    except Exception as e:
        print(f"发送数据时出错: {e}")

# socket接收数据（）
def receive_data(client_socket):
    """从服务器接收数据"""
    try:
        # 使用 select 监听 socket 是否可读
        readable, _, _ = select.select([client_socket], [], [], 0.05)
        if readable:
            # 接收服务器的确认信息
            response = client_socket.recv(136)
            if not response:
                print(f"和服务端断开连接，尝试重新连接服务端...")
                # 连接到服务器
                try:
                    client_socket.connect((server_ip, server_port))
                except BlockingIOError:
                    # 在非阻塞模式下，连接可能会立即失败
                    print("socket连接失败...")
                    pass
            
            # 解包接收到的数据
            try:
                global get_check1
                global get_speed
                global get_mode
                global get_doit
                global get_longitude
                global get_latitude
                global get_altitude
                global get_check2
                print(f"before recdive doit :{get_doit}")
                unpacked_data = struct.unpack('>17d', response)
                get_check1, get_speed, get_mode, get_doit, \
                get_longitude[0], get_latitude[0], get_altitude[0], \
                get_longitude[1], get_latitude[1], get_altitude[1], \
                get_longitude[2], get_latitude[2], get_altitude[2], \
                get_longitude[3], get_latitude[3], get_altitude[3],get_check2 = unpacked_data
                print(f"recdive doit :{get_doit}")
                # print(f"收到来自服务端的数据:")
                # print(f"  经度: {get_longitude}, 纬度: {get_latitude}, 高度: {get_altitude}")
                # print(f"  速度: {get_speed}, 飞行模式: {get_mode}")
            except struct.error as e:
                print(f"解包数据时发生错误: {e}")
    except BlockingIOError:
        print("接收数据时阻塞，请稍后再试。")
    except Exception as e:
        print(f"接收数据时出错: {e}")

# 尝试连接服务器
def connect_to_server():
    # 连接到服务器
    while True:
        try:
            print("socket连接中...")
            client_socket.connect((server_ip, server_port))
            print("socket连接成功...")
            break
        except BlockingIOError:
            # 在非阻塞模式下，连接可能会立即失败
            print("socket连接失败...")
            pass


"""ros相关开始"""######################################################
send_dronedatas = Flydatas()       # ros中用来发送给无人机代码订阅的消息
get_dronedatas = Flydatas()        # ros中订阅无人机消息的变量
send_dronedatas.header.frame_id = "client_to_drone"
# 提供一个将存储发送消息的变量全部初始化的函数，方便使用完数据后初始化
def send_dronedatas_init():
    global send_dronedatas
    # # 初始化 header
    # send_dronedatas.header = Header()
    # send_dronedatas.header.stamp = rospy.Time.now()
    # send_dronedatas.header.frame_id = "client_to_drone"
    # # 初始化 flight_state
    # send_dronedatas.flight_state = State()
    # send_dronedatas.flight_state.armed = False
    # send_dronedatas.flight_state.connected = False
    # send_dronedatas.flight_state.mode = ""
    # 初始化 gps 数组
    send_dronedatas.gps = [NavSatFix() for _ in range(4)]
    for gps in send_dronedatas.gps:
        gps.latitude = 0.0
        gps.longitude = 0.0
        gps.altitude = 0.0
        gps.status.status = -1
        gps.status.service = 1
    # # 初始化 pos
    # send_dronedatas.pos = PoseStamped()
    # send_dronedatas.pos.header = send_dronedatas.header
    # send_dronedatas.pos.pose.position.x = 0.0
    # send_dronedatas.pos.pose.position.y = 0.0
    # send_dronedatas.pos.pose.position.z = 0.0
    # send_dronedatas.pos.pose.orientation.w = 1.0  # 单位四元数
    # 初始化 doit
    send_dronedatas.doit = False
# 提供一个将存储接收消息的变量全部初始化的函数，方便使用完数据后初始化
def get_dronedatas_init():
    global get_dronedatas
    # # 初始化 flight_state
    # get_dronedatas.flight_state = State()
    # get_dronedatas.flight_state.armed = False
    # get_dronedatas.flight_state.connected = False
    # get_dronedatas.flight_state.mode = ""
    # 初始化 gps 数组
    get_dronedatas.gps = [NavSatFix() for _ in range(4)]
    for gps in get_dronedatas.gps:
        gps.latitude = 0.0
        gps.longitude = 0.0
        gps.altitude = 0.0
        gps.status.status = -1
        gps.status.service = 1
    # # 初始化 pos
    # get_dronedatas.pos = PoseStamped()
    # get_dronedatas.pos.header = get_dronedatas.header
    # get_dronedatas.pos.pose.position.x = 0.0
    # get_dronedatas.pos.pose.position.y = 0.0
    # get_dronedatas.pos.pose.position.z = 0.0
    # get_dronedatas.pos.pose.orientation.w = 1.0  # 单位四元数
    # 初始化 doit
    get_dronedatas.doit = False


# 回调函数
def callback(data):
    rospy.loginfo("I heard %s", data.data)

def flydatas_callback(msg):
    global get_dronedatas
    get_dronedatas = msg
    # print(f"flydatas_cllback lat is {get_dronedatas.gps[0].longitude}")
    # print(f"flydatas_callback doit is {get_dronedatas.doit}")


"""ros相关结束"""######################################################

if __name__ == "__main__":
    try :
        # 尝试连接到服务器
        connect_to_server()
        # ros初始化
        rospy.init_node('client_node', anonymous=True)
        pub = rospy.Publisher('/pub_flydatas', Flydatas, queue_size=10)
        pub11 = rospy.Publisher('chatter', String, queue_size=10)
        rospy.Subscriber("/sub_flydatas", Flydatas, flydatas_callback)
        rospy.Subscriber("/chatter", String, callback)
        rate10 = rospy.Rate(10)  # 设定频率为10Hz

        while not rospy.is_shutdown():
            rate10.sleep()  # 让循环按照设定的频率休眠（和回调函数的刷新速率无关）
            # 这里可以放置其他代码或处理逻辑 
            # 要发布的消息内容
            # hello_str = "Hello ROS from outside workspace! %s" % rospy.get_time()
            # rospy.loginfo(hello_str)
            # # 发布消息
            # pub11.publish(hello_str)
            msg = Flydatas()

            # Filling Header
            msg.header = Header()
            msg.header.stamp = rospy.Time.now()
            msg.header.frame_id = "base_link"

            # Filling State (replace with actual values)
            msg.flight_state = State()
            msg.flight_state.mode = "OFFBOARD"
            msg.flight_state.armed = True

            # Filling GPS data (replace with actual GPS values)
            msg.gps = []  # Clear the list to ensure no extra items are present
            for i in range(4):  # Ensure exactly 4 items are added
                gps = NavSatFix()# 初始lat：47.397742，lon：8.545593,alt:535.365917
                # gps.latitude = 47.397742 + i * 0.0001  # Example latitudes
                # gps.longitude = 8.545693 + i * 0.0001  # Example longitudes
                # gps.altitude = 550.365917 + i  # Example altitudes
                msg.gps.append(gps)

            msg.gps[0].latitude = 47.397742
            msg.gps[0].longitude = 8.545593
            msg.gps[0].altitude = 565.365917
            # 47°23'51.4"N 8°32'52.2"E
            msg.gps[1].latitude = 47.398159
            msg.gps[1].longitude = 8.547984
            msg.gps[1].altitude = 565.365917
            # 47°23'51.9"N 8°32'44.1"E
            msg.gps[2].latitude = 47.396556
            msg.gps[2].longitude =  8.546871
            msg.gps[2].altitude = 565.365917
            # 初始lat：47.397742，lon：8.545593,alt:535.365917
            msg.gps[3].latitude = 47.397742
            msg.gps[3].longitude = 8.545593
            msg.gps[3].altitude = 565.365917

            msg.doit = True
            pub.publish(msg)


            # # 模拟要发送的数据
            # send_check1 = 888.0
            # send_speed = 1.0       # 速度
            # send_mode = 0.0        # 飞行模式/任务：1.0表示...2.0表示...
            # send_doit = 1.0        # 执行确认位(1.0表示有效，0.0表示无效)
            # send_longitude[0] = 110.923632   # GPS定位1
            # send_latitude[0] = 21.676893
            # send_altitude[0] = 25
            # send_longitude[1] = 110.923732   # GPS定位2
            # send_latitude[1] = 21.676893
            # send_altitude[1] = 20
            # send_longitude[2] = 110.923832   # GPS定位3
            # send_latitude[2] = 21.676893
            # send_altitude[2] = 23
            # send_longitude[3] = 110.923632   # GPS定位4
            # send_latitude[3] = 21.676893
            # send_altitude[3] = 25
            # send_check2 = 666.0
            # # (校验位(值为888.0有效),速度,飞行模式/任务,执行位(1.0表示有效，0.0表示无效),经度1, 纬度1,高度1,经度2, 纬度2,高度2,经度3, 纬度3,高度3,经度4, 纬度4, 高度4)
            # data_to_send = [send_check1, send_speed, send_mode,send_doit,\
            #                 send_longitude[0], send_latitude[0],send_altitude[0],\
            #                 send_longitude[1], send_latitude[1],send_altitude[1],\
            #                 send_longitude[2], send_latitude[2],send_altitude[2],\
            #                 send_longitude[3], send_latitude[3],send_altitude[3],send_check2]
            # # 调用发送函数
            # send_data(client_socket, data_to_send)

            # # 调用接收函数
            # receive_data(client_socket)
            print(f"get_check1:{get_check1},get_check2:{get_check2},get_mode:{get_mode},get_doit:{get_doit}")
            get_check1 == 888.0
            get_check2 == 666.0
            get_mode == 1.0
            get_doit == 1.0
            # 如果获取到GPS任务数据，则回应并发布对应的话题
            if get_check1 == 888.0 and get_check2 == 666.0 and get_mode == 1.0 and get_doit == 1.0 :
                print("gps is coming")
                pub.publish(msg)
                send_dronedatas_init()     # 初始化gps和doit数据
                send_dronedatas.gps[0].longitude = get_longitude[0]
                send_dronedatas.gps[0].latitude = get_latitude[0]
                send_dronedatas.gps[0].altitude = get_altitude[0]
                send_dronedatas.gps[1].longitude = get_longitude[1]
                send_dronedatas.gps[1].latitude = get_latitude[1]
                send_dronedatas.gps[1].altitude = get_altitude[1]
                send_dronedatas.gps[2].longitude = get_longitude[2]
                send_dronedatas.gps[2].latitude = get_latitude[2]
                send_dronedatas.gps[2].altitude = get_altitude[2]
                send_dronedatas.gps[3].longitude = get_longitude[3]
                send_dronedatas.gps[3].latitude = get_latitude[3]
                send_dronedatas.gps[3].altitude = get_altitude[3]
                print(f"get_doit:{get_doit}")
                start_time = time.time()
                send_datas_init()           # 发送数据前初始化发送的数据，再将需要的赋值
                # 回应服务端（收到数据后的都要回应）
                while time.time() - start_time < 0.5:    # 发送数据0.5s，告知服务端以收到数据，要求他停止发送
                    send_doit = 1.0         # 表示已收到数据
                    data_to_send = [send_check1, send_speed, send_mode,send_doit,\
                                send_longitude[0], send_latitude[0],send_altitude[0],\
                                send_longitude[1], send_latitude[1],send_altitude[1],\
                                send_longitude[2], send_latitude[2],send_altitude[2],\
                                send_longitude[3], send_latitude[3],send_altitude[3],send_check2]
                    send_data(client_socket,data_to_send)
                    time.sleep(0.1)    # 休眠0.1秒

                # 发布话题控制无人机 time.time() - start_time < 2
                start_time = time.time()
                # while not rospy.is_shutdown():       # 最多等待2s
                #     # 更新时间和发布次数后发布
                #     send_dronedatas.header.seq += send_dronedatas.header.seq
                #     send_dronedatas.header.stamp = rospy.Time.now()
                #     pub.publish(send_dronedatas)
                #     print(f"get_dronedatas.doit:{get_dronedatas.doit}")
                #     if get_dronedatas.doit == True:
                #         # 命令发送成功后将服务端发过来的数据清零，处理完了就清零，避免残留数据对之后代码产生影响
                #         get_datas_to_0()
                #         print("无人机已接收到GPS任务，退出任务消息发布。")
                #         break
                #     rate10.sleep()
                # print("发布退出了")
            get_datas_to_0()        # 使用完后清零数据

    except rospy.ROSInterruptException:
        print("接收到 ROS 关闭请求，正在退出...")
    except KeyboardInterrupt:
        print("手动中断，正在退出...")
    except Exception as e:
        print(f"发生错误: {e}")
    finally:
        # 在这里做一些清理工作
        print("清理工作完成，程序退出。")
        sys.exit()

    # python中不需要手动刷新回调函数，回调函数的执行速率取决于发布速率(只要节点不退出)
    # rospy.spin() # 保持节点活跃，有消息时调用回调函数

